
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavproxy_common.c
  * @author     baiyang
  * @date       2021-8-25
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <rtconfig.h>

#include "mavproxy_monitor.h"

#include "mavproxy.h"
#include "mavcmd.h"

#include <copter/fms.h>
#include <parameter/param.h>
#include <common/microbee.h>

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_imu/sensor_imu.h>
#include <sensor_gps/sensor_gps.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#endif

#include <board_config/borad_config.h>
#include <notify/notify.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void mavproxy_update_sensor_status_flags();
static void update_vehicle_sensor_status_flags(void);
/*----------------------------------variable----------------------------------*/
static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;

static uint32_t control_sensors_present;
static uint32_t control_sensors_enabled;
static uint32_t control_sensors_health;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void mavproxy_send_command_ack(uint16_t command, uint8_t result, 
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

void mavproxy_send_command_ack_sysid(uint16_t command, uint8_t result,
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};

    mavlink_msg_command_ack_encode(msg->sysid, msg->compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

#if 0
void mavproxy_proc_command(mavlink_command_long_t* command,
    mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;
    bool ignore = true;

    switch (command->command) {
    case MAV_CMD_PREFLIGHT_CALIBRATION: {
        ignore = false;

        if (command->param1 == 1) { // calibration gyr
            mavcmd_set(MAVCMD_CALIBRATION_GYR, NULL);
        } else if (command->param2 == 1) { // calibration mag
            mavcmd_set(MAVCMD_CALIBRATION_MAG, NULL);
        } else if (command->param5 == 1) { // calibration acc
            mavcmd_set(MAVCMD_CALIBRATION_ACC, NULL);
        } else if (command->param5 == 2) { // calibration level
            // mavproxy_send_statustext_msg(CAL_START_LEVEL, msg);
        } else {
            /* all 0 command, cancel current process */
        }

        result = MAV_CMD_ACK_OK | MAV_CMD_ACK_ENUM_END;
        break;
    }

    default:
        break;
    }

    if (!ignore) {
        mavproxy_send_command_ack(command->command, result, msg);
    }
}
#endif

// return MAV_TYPE corresponding to frame class
MAV_TYPE mavproxy_get_frame_mav_type()
{
    switch ((motor_frame_class)PARAM_GET_INT8(VEHICLE,FRAME_CLASS)) {
        case MOTOR_FRAME_QUAD:
        case MOTOR_FRAME_UNDEFINED:
            return MAV_TYPE_QUADROTOR;
        case MOTOR_FRAME_HEXA:
        case MOTOR_FRAME_Y6:
            return MAV_TYPE_HEXAROTOR;
        case MOTOR_FRAME_OCTA:
        case MOTOR_FRAME_OCTAQUAD:
            return MAV_TYPE_OCTOROTOR;
        case MOTOR_FRAME_HELI:
        case MOTOR_FRAME_HELI_DUAL:
        case MOTOR_FRAME_HELI_QUAD:
            return MAV_TYPE_HELICOPTER;
        case MOTOR_FRAME_TRI:
            return MAV_TYPE_TRICOPTER;
        case MOTOR_FRAME_SINGLE:
        case MOTOR_FRAME_COAX:
        case MOTOR_FRAME_TAILSITTER:
            return MAV_TYPE_COAXIAL;
        case MOTOR_FRAME_DODECAHEXA:
            return MAV_TYPE_DODECAROTOR;
        case MOTOR_FRAME_DECA:
            return MAV_TYPE_DECAROTOR;
    }
    // unknown frame so return generic
    return MAV_TYPE_GENERIC;
}

MAV_MODE mavproxy_base_mode()
{
    Motors_HandleTypeDef *motors = fms.motors;

    uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
    // work out the base_mode. This value is not very useful
    // for APM, but we calculate it as best we can so a generic
    // MAVLink enabled ground station can work out something about
    // what the MAV is up to. The actual bit values are highly
    // ambiguous for most of the APM flight modes. In practice, you
    // only get useful information from the custom_mode, which maps to
    // the APM flight mode and has a well defined meaning in the
    // ArduPlane documentation
    switch (mode_number(fms.flightmode)) {
    case AUTO:
    case AUTO_RTL:
    case RTL:
    case LOITER:
    case AVOID_ADSB:
    case FOLLOW:
    case GUIDED:
    case CIRCLE:
    case POSHOLD:
    case BRAKE:
    case SMART_RTL:
        _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
        // APM does in any mode, as that is defined as "system finds its own goal
        // positions", which APM does not currently do
        break;
    default:
        break;
    }

    // all modes except INITIALISING have some form of manual
    // override if stick mixing is enabled
    _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;

    // we are armed if we are not initialising
    if (motors != NULL && motors->_armed) {
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    }

    // indicate we have set a custom mode
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    return (MAV_MODE)_base_mode;
}

MAV_STATE mavproxy_vehicle_system_status()
{
    // set system as critical if any failsafe have triggered
    if (fms.failsafe.radio || 
        fms.failsafe.gcs || 
        fms.failsafe.ekf || 
        fms.failsafe.terrain || 
        fms.failsafe.adsb)  {
        return MAV_STATE_CRITICAL;
    }

    if (fms.ap.land_complete) {
        return MAV_STATE_STANDBY;
    }

    return MAV_STATE_ACTIVE;
}

uint32_t mavproxy_custom_mode()
{
    return (uint32_t)mode_number(fms.flightmode);
}

void mavproxy_send_banner()
{
    // mark the firmware version in the tlog
    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", THISFIRMWARE);

    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s: %s",
                  "OS", "RT-Thread");

    // send system ID if we can
    //char sysid[40];
    //mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", "");
}

void mavproxy_get_sensor_status_flags(uint32_t *present,
                                  uint32_t *enabled,
                                  uint32_t *health)
{
    mavproxy_update_sensor_status_flags();

    *present = control_sensors_present;
    *enabled = control_sensors_enabled;
    *health = control_sensors_health;
}

void mavproxy_update_sensor_status_flags()
{
    control_sensors_present = 0;
    control_sensors_enabled = 0;
    control_sensors_health = 0;

    
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    control_sensors_present |= MAV_SYS_STATUS_AHRS;
    control_sensors_enabled |= MAV_SYS_STATUS_AHRS;
    control_sensors_health |= MAV_SYS_STATUS_AHRS;

    if (sensor_compass_available()) {
      control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
      control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
    }
    if (sensor_compass_available() && sensor_compass_healthy()) {
      control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
    }

    control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    if (sensor_baro_all_healthy()) {
      control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    }

    if (sensor_gps_status() > NO_GPS) {
      control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
      control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
    }
    if (sensor_gps_is_healthy(gps.primary_instance) && sensor_gps_status() >= NO_FIX) {
      control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
    }

    control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
    control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
    if (!sensor_imu_calibrating()) {
      control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
      control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
      if (sensor_imu_get_accel_health_all()) {
          control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
      }
      if (sensor_imu_get_gyro_health_all() && sensor_imu_gyro_calibrated_ok_all()) {
          control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
      }
  }

#endif

    control_sensors_present |= MAV_SYS_STATUS_LOGGING;
    control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
    control_sensors_health |= MAV_SYS_STATUS_LOGGING;

    // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
    control_sensors_present |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
    if (brd_safety_switch_state() != SAFETY_DISARMED) {
      control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
    }
    control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    // always show EKF type 10 as healthy. This prevents spurious error
    // messages in xplane and other simulators that use EKF type 10
    control_sensors_present |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
    control_sensors_enabled |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
    control_sensors_health |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
#endif

    // give GCS status of prearm checks. This is enabled if any arming checks are enabled.
    // it is healthy if armed or checks are passing
    control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK;
    if (fms.arming.checks_to_perform) {
        control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK;
        if (brd_safety_switch_state() || notify_flags.pre_arm_check) {
            control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK;
        }
    }

  update_vehicle_sensor_status_flags();
}

static void update_vehicle_sensor_status_flags(void)
{
    control_sensors_present |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    control_sensors_enabled |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    control_sensors_health |=
        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
        MAV_SYS_STATUS_SENSOR_YAW_POSITION;

    const ap_t ap = fms.ap;

    if (ap.rc_receiver_present) {
        control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
    }
    if (ap.rc_receiver_present && !fms.failsafe.radio) {
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
    }

    // update flightmode-specific flags:
    control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
    control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

    if (fms.flightmode != NULL) {
        switch (mode_number(fms.flightmode)) {
        case AUTO:
        case AUTO_RTL:
        case AVOID_ADSB:
        case GUIDED:
        case LOITER:
        case RTL:
        case CIRCLE:
        case LAND:
        case POSHOLD:
        case BRAKE:
        case THROW:
        case SMART_RTL:
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
            break;
        case ALT_HOLD:
        case GUIDED_NOGPS:
        case SPORT:
        case AUTOTUNE:
        case FLOWHOLD:
            control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
            control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
            break;
        default:
            // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
            break;
        }
    }

    control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    // only mark propulsion healthy if all of the motors are producing
    // nominal thrust
    if (fms.motors != NULL && !((MotorsMat_HandleTypeDef* )fms.motors)->_motor_lost_index) {
        control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROPULSION;
    }
}

void mavproxy_send_accelcal_vehicle_position(uint32_t position)
{
    mavlink_message_t msg;
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_command_long_t command_long_t = {.param1 = (float) position, 
                                             .param2 = 0.0f,
                                             .param3 = 0.0f,
                                             .param4 = 0.0f,
                                             .param5 = 0.0f,
                                             .param6 = 0.0f,
                                             .param7 = 0.0f,
                                             .command = MAV_CMD_ACCELCAL_VEHICLE_POS,
                                             .target_system = 0,
                                             .target_component = 0,
                                             .confirmation = 0};

    mavlink_msg_command_long_encode(sysid_this_mav.sysid, sysid_this_mav.compid, &msg, &command_long_t);
    mavproxy_send_immediate_msg(&msg, 1);
}


static uint64_t capabilities()
{
    uint64_t ret = MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
        MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION |
        MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
        MAV_PROTOCOL_CAPABILITY_MISSION_INT |
        MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
        MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
        MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
        MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
        MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;

    ret |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
    ret |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
    //ret |= MAV_PROTOCOL_CAPABILITY_FTP;
    
    return ret;
}

/*
  send AUTOPILOT_VERSION packet
 */
void mavproxy_send_autopilot_version()
{
    mavlink_message_t msg = {0};

    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    uint32_t flight_sw_version;
    uint32_t middleware_sw_version = 0;
    uint32_t board_version = 0;
    uint16_t vendor_id = 0;
    uint16_t product_id = 0;
    uint64_t uid = 0;
    uint8_t  uid2[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_UID2_LEN] = {0x00,0x48,0x00,0x3d,0x32,0x35,0x51,0x0f,0x31,0x36,0x33,0x39};

    uint8_t uid_len = sizeof(uid2); // taken as reference and modified
                                    // by following call:
    
    flight_sw_version = 4 << (8 * 3) | \
                        1 << (8 * 2) | \
                        0 << (8 * 1) | \
                        (uint32_t)(FIRMWARE_VERSION_TYPE_DEV) << (8 * 0);

    mavlink_autopilot_version_t ver = {
        .capabilities = capabilities(),
        .flight_sw_version = flight_sw_version,
        .middleware_sw_version = middleware_sw_version,
        .os_sw_version = 0,
        .board_version = board_version,
        .flight_custom_version = "cb570c06",
        .middleware_custom_version = {0},
        .os_custom_version = "9d8e2b96",
        .vendor_id = vendor_id,
        .product_id =product_id,
        .uid = uid,
        .uid2 = {0x00,0x48,0x00,0x3d,0x32,0x35,0x51,0x0f,0x31,0x36,0x33,0x39},
    };

    mavlink_msg_autopilot_version_encode(sysid_this_mav.sysid, sysid_this_mav.compid, &msg, &ver);
    mavproxy_send_immediate_msg(&msg, 1);
}

/*------------------------------------test------------------------------------*/


